#include "./status_task.h"
#include "../pearl_driver/sensor.h"
#include "../pub_driver/cmd_can.h"
#include "../pearl_driver/move_motor.h"
#include "../pearl_driver/updown_motor.h"

namespace pearl_task {

StatusTask statusTask;

using namespace pub_driver;
using namespace pearl_driver;
using namespace conn;

static constexpr uint16_t VERSION = 0x04;

void StatusTask::run() {

    for (;;) {

        uint8_t sw2 = moveMotor.is_alm() |
            (updownMotor.is_alm() << 1);

        cmdCAN.notify(
            addr::IPC,
            proto::pearl::STATUS_UPLOAD,
            VERSION, 
            sensor.read(),
            sw2,
            sensor.adc_value()
        );

        osDelay(500);
    }
}

}
